/* 编码器库 - 双旋钮示例
 * http://www.pjrc.com/teensy/td_libs_Encoder.html
 *
 * 此示例代码属于公共领域。
 */
// Russ76添加了Adafruit BNO055代码
// 红线连接3.3V，黑线接地，黄线连接SCL 19，蓝线连接SDA 18
/*
 * rosserial 发布/订阅示例
 * 打印"hello world!"并切换LED
 */

#include <ros.h>
#include "ros/time.h"
//#include <std_msgs/Int32.h>
#include <diffbot_msgs/Encoders.h>
#include <std_msgs/Empty.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include "sensor_msgs/Imu.h"
#include <Encoder.h>

// 编码器设置
// 修改这些引脚编号以匹配连接到编码器的引脚。
//   最佳性能：两个引脚都具有中断功能
//   良好性能：仅第一个引脚具有中断功能
//   低性能：两个引脚都没有中断功能
Encoder encoderLeft(5, 6);  // 默认引脚 5, 6
Encoder encoderRight(7, 8); // 默认引脚 7, 8
//   避免使用带有LED的引脚
char imu_link[] = "imu";

ros::NodeHandle  nh;

// 检查I2C设备地址并修正下面的行（默认地址为0x29或0x28）
//                                   id, 地址
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
sensor_msgs::Imu Imu_msg; 
ros::Publisher imu_pub("imu/data_raw", &Imu_msg);

// ROS订阅器设置，用于将两个编码器重置为零
void resetCallback( const std_msgs::Empty& reset)
{
  //digitalWrite(13, HIGH-digitalRead(13));   // 闪烁LED
  // 将两个编码器重置为零
  encoderLeft.write(0);
  encoderRight.write(0);
  nh.loginfo("将两个车轮编码器重置为零");
}

ros::Subscriber<std_msgs::Empty> sub_reset("reset", resetCallback);

// ROS发布器设置，用于发布左右编码器计数
// 使用自定义的编码器计数消息，定义了一个包含两个整数的数组
diffbot_msgs::Encoders encoder_ticks;
ros::Publisher pub_encoders("encoder_ticks", &encoder_ticks);

void setup() 
{
  nh.initNode();
  nh.advertise(pub_encoders);
  nh.subscribe(sub_reset);
  nh.advertise(imu_pub);
  while (!nh.connected())
  {
    nh.spinOnce();
  }
  nh.loginfo("初始化DiffBot车轮编码器");
  std_msgs::Empty reset;
  resetCallback(reset);
  delay(1);
  
    /* 初始化传感器 */
  if (!bno.begin())
  {
    /* 检测BNO055时出现问题...检查连接 */
    Serial.print("糟糕，未检测到BNO055...请检查接线或I2C地址！");
    while (1);
  }

  delay(100);
  bno.setExtCrystalUse(true);
}

long positionLeft  = -999;
long positionRight = -999;

void loop() {  
  long newLeft, newRight;
  newLeft = encoderLeft.read();
  newRight = encoderRight.read();

  encoder_ticks.ticks[0] = newLeft;
  encoder_ticks.ticks[1] = newRight;
  pub_encoders.publish(&encoder_ticks);
  nh.spinOnce();
  
       
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  imu::Quaternion quat = bno.getQuat();
  
  //nh.loginfo("现在在主循环中。");
	
  // http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/Imu.html
  sensor_msgs::Imu imu_msg;
    imu_msg.header.frame_id = imu_link;
    imu_msg.header.stamp = nh.now();
    imu_msg.orientation.x = quat.w();
    imu_msg.orientation.y = quat.x();
    imu_msg.orientation.z = quat.y();
    imu_msg.orientation.w = quat.z();
    imu_msg.linear_acceleration.x = (euler.x());
    imu_msg.linear_acceleration.y = (euler.y());
    imu_msg.linear_acceleration.z = (euler.z());
    imu_msg.angular_velocity.x = (gyro.x());
    imu_msg.angular_velocity.y = (gyro.y());
    imu_msg.angular_velocity.z = (gyro.z());
  // https://github.com/Russ76/ros_mpu6050_node-1/blob/master/src/mpu6050_node.cpp
  imu_pub.publish(&imu_msg);
		
  
  // 在工作PC上至少使用3毫秒延迟，在树莓派上使用5毫秒延迟
  // 延迟太低会导致rosserial出现类似以下错误：
  // [INFO]: 主题ID和消息的校验和错误
  // [INFO]: 消息长度校验和错误，长度4，丢弃消息。
  // [ERROR]: 数据包中的协议版本不匹配(b'\x00'): 失去同步或rosserial_python与rosserial客户端来自不同的ros版本
  // [INFO]: 无法识别客户端的协议版本，期望Rev 1 (rosserial 0.5+)
  // [INFO]: 主题ID和消息的校验和错误
  // [WARN]: 最后读取步骤：消息长度
  // [WARN]: 运行循环错误：串口读取失败：返回值过短(期望3字节，实际接收2字节)。
  // [INFO]: 请求主题...
  // [ERROR]: 与设备失去同步，重新启动...
  // [INFO]: 请求主题...
  delay(10);
  // 注意 https://www.arduino.cc/reference/en/language/functions/time/delay/:
  // 当delay()函数控制Atmega芯片时，某些操作仍会继续进行，
  // 因为delay函数不会禁用中断。
  // RX引脚上出现的串行通信会被记录，PWM(analogWrite)值和引脚状态会保持，
  // 中断将正常工作。
  
  if (newLeft != positionLeft || newRight != positionRight) {
    //String str = "左轮 = " + String(newLeft) + ", 右轮 = " + String(newRight);
    //nh.loginfo(str);
    positionLeft = newLeft;
    positionRight = newRight;
  }
}
